clear all;

% top view region of AVM image
Region_TL_X = 21;
Region_TL_Y = 8;
Region_BR_X = 339;
Region_BR_Y = 466;

IMG_Height = 459;
IMG_Width = 319;

% Each camera's position in the top view image (cropped image)
F_CAM_X = 160;  F_CAM_Y = 80;
L_CAM_X = 113;  L_CAM_Y = 161;
R_CAM_X = 208;  R_CAM_Y = 161;
B_CAM_X = 160;  B_CAM_Y = 324;

load('PH_LUT_20140120.mat');

% select DB
% DB_name = 'DB0_V-Static_O-Single\';   image_num = 497;
DB_name = 'DB1_V-Static_O-Two\';    image_num = 3592;
% DB_name = 'DB2_V-Parking_O-Single\';    image_num = 2405;
% DB_name = 'DB3_V-Parking_O-Single\';    image_num = 2809;
% DB_name = 'DB4_V-Static_O-Three\';    image_num = 1726;
% DB_name = 'DB5_V-Static_O-Three\';    image_num = 3143;
% DB_name = 'DB6_V-Static_O-Three\';    image_num = 3725;

for i = 3 : image_num
    % Load two successive image
    cur_img_name = [DB_name, int2str(i), '.jpg'];         cur_img = imread(cur_img_name);
    pre_img_name = [DB_name, int2str(i-2), '.jpg'];       pre_img = imread(pre_img_name);
    figure_name = ['PH_Result_', int2str(i), '.jpg'];
    
    % Crop image
    cur_crop_img = cur_img(Region_TL_Y:Region_BR_Y, Region_TL_X:Region_BR_X);
    pre_crop_img = pre_img(Region_TL_Y:Region_BR_Y, Region_TL_X:Region_BR_X);

%%    
    % Frame Difference
    diff_img = abs(cur_crop_img - pre_crop_img);
    temp_img = zeros(459, 319, 'uint8');
    bin_img = zeros(459, 319, 'uint8');
    
    bin_TH = 20;

%     % Simple thresholding
%     for y = 1 : IMG_Height
%         for x = 1 : IMG_Width
%             if (diff_img(y,x) > bin_TH)
%                 bin_img(y,x) = 255;
%             else
%                 bin_img(y,x) = 0;
%             end
%         end
%     end

    
    % Opening
    for y = 1+3 : IMG_Height-3
        for x = 1+3 : IMG_Width-3
            if (diff_img(y,x) < bin_TH) || (diff_img(y-1,x) < bin_TH) || (diff_img(y+1,x) < bin_TH) || (diff_img(y,x-1) < bin_TH) || (diff_img(y,x+1) < bin_TH)
                temp_img(y,x) = 0;
            else
                temp_img(y,x) = 255;
            end
        end
    end
    for y = 1+3 : IMG_Height-3
        for x = 1+3 : IMG_Width-3
            if temp_img(y,x) || temp_img(y-1,x) || temp_img(y+1,x) || temp_img(y,x-1) || temp_img(y,x+1)
                bin_img(y,x) = 255;
            else
                bin_img(y,x) = 0;
            end
        end
    end
    
    
%%     
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Front
    Front_PH = zeros(1, 15);
    for y = 1 : F_CAM_Y
        % PH Extract
        for x = Front_LUT(y,1) : Front_LUT(y,15)
            if (x < IMG_Width)
                if (bin_img(y,x) > 0)
                    if     ( x < Front_LUT(y,1) )
                        Front_PH(1) = Front_PH(1) + 1;
                    end
                    for line_index = 2 : 15
                        if (x >= Front_LUT(y,line_index-1)) && (x < Front_LUT(y,line_index))
                            Front_PH(line_index) = Front_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 15
            cur_crop_img(y, Front_LUT(y,line_index)) = 255;
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Front and Left Corner
    FL_Corner_PH = zeros(1, 5);
    for x = 1 : F_CAM_X
        % PH Extract
        for y = FL_Corner_LUT(x,1) : FL_Corner_LUT(x,5)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if     ( y < FL_Corner_LUT(x,1) )
                        FL_Corner_PH(1) = FL_Corner_PH(1) + 1;
                    end
                    for line_index = 2 : 5
                        if (y >= FL_Corner_LUT(x,line_index-1)) && (y < FL_Corner_LUT(x,line_index))
                            FL_Corner_PH(line_index) = FL_Corner_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 5
            cur_crop_img(FL_Corner_LUT(x,line_index), x) = 255;
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Front and Right Corner
    FR_Corner_PH = zeros(1, 5);
    for x = F_CAM_X : IMG_Width
        % PH Extract
        for y = FR_Corner_LUT(IMG_Width-x+1,1) : FR_Corner_LUT(IMG_Width-x+1,5)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if ( y < FR_Corner_LUT(IMG_Width-x+1,1) )
                        FR_Corner_PH(1) = FR_Corner_PH(1) + 1;
                    end
                    for line_index = 2 : 5
                        if (y >= FR_Corner_LUT(IMG_Width-x+1,line_index-1)) && (y < FR_Corner_LUT(IMG_Width-x+1,line_index))
                            FR_Corner_PH(line_index) = FR_Corner_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 5
            cur_crop_img(FR_Corner_LUT(IMG_Width-x+1,line_index), x) = 255;
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Back
    Back_PH = zeros(1, 15);
    for y = B_CAM_Y : IMG_Height
        % PH Extract
        for x = Back_LUT(IMG_Height-y+1,1) : Back_LUT(IMG_Height-y+1,15)
            if (x < IMG_Width)
                if (bin_img(y,x) > 0)
                    if     ( x < Back_LUT(IMG_Height-y+1,1) )
                        Back_PH(1) = Back_PH(1) + 1;
                    end
                    for line_index = 2 : 15
                        if (x >= Back_LUT(IMG_Height-y+1,line_index-1)) && (x < Back_LUT(IMG_Height-y+1,line_index))
                            Back_PH(line_index) = Back_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw        
        for line_index = 1 : 15
            cur_crop_img(y, Back_LUT(IMG_Height-y+1,line_index)) = 255;
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Back and Left Corner
    BL_Corner_PH = zeros(1, 5);
    for x = 1 : B_CAM_X
        % PH Extract
        for y = BL_Corner_LUT(x,1) : BL_Corner_LUT(x,5)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if     ( y < BL_Corner_LUT(x,1) )
                        BL_Corner_PH(1) = BL_Corner_PH(1) + 1;
                    end
                    for line_index = 2 : 5
                        if (y >= BL_Corner_LUT(x,line_index-1)) && (y < BL_Corner_LUT(x,line_index))
                            BL_Corner_PH(line_index) = BL_Corner_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw    
        for line_index = 1 : 5
            cur_crop_img(BL_Corner_LUT(x,line_index), x) = 255;
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Back and Right Corner
    BR_Corner_PH = zeros(1, 5);
    for x = B_CAM_X : IMG_Width
        % PH Extract
        for y = BR_Corner_LUT(IMG_Width-x+1,1) : BR_Corner_LUT(IMG_Width-x+1,5)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if     ( y < BR_Corner_LUT(IMG_Width-x+1,1) )
                        BR_Corner_PH(1) = BR_Corner_PH(1) + 1;
                    end
                    for line_index = 2 : 5
                        if (y >= BR_Corner_LUT(IMG_Width-x+1,line_index-1)) && (y < BR_Corner_LUT(IMG_Width-x+1,line_index))
                            BR_Corner_PH(line_index) = BR_Corner_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 5
            cur_crop_img(BR_Corner_LUT(IMG_Width-x+1,line_index), x) = 255;
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Left
    Left_PH = zeros(1, 19);
    for x = 1 : L_CAM_X
        % PH Extract
        for y = Left_LUT(x,1) : Left_LUT(x,19)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if     ( y < Left_LUT(x,1) )
                        Left_PH(1) = Left_PH(1) + 1;
                    end
                    for line_index = 2 : 19
                        if (y >= Left_LUT(x,line_index-1)) && (y < Left_LUT(x,line_index))
                            Left_PH(line_index) = Left_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 19
            cur_crop_img(Left_LUT(x,line_index), x) = 255;
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Extract Polar Histogram @ Right
    Right_PH = zeros(1, 19);
    for x = R_CAM_X : IMG_Width
        % PH Extract
        for y = Right_LUT(IMG_Width-x+1,1) : Right_LUT(IMG_Width-x+1,19)
            if (y < IMG_Height)
                if (bin_img(y,x) > 0)
                    if     ( y < Right_LUT(IMG_Width-x+1,1) )
                        Right_PH(1) = Right_PH(1) + 1;
                    end
                    for line_index = 2 : 19
                        if (y >= Right_LUT(IMG_Width-x+1,line_index-1)) && (y < Right_LUT(IMG_Width-x+1,line_index))
                            Right_PH(line_index) = Right_PH(line_index) + 1;
                        end
                    end
                end
            end
        end
        % Line Draw
        for line_index = 1 : 19
            cur_crop_img(Right_LUT(IMG_Width-x+1,line_index), x) = 255;
        end
    end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Enroll several PHs
    Total_PH = zeros(1, 89);
    for k = 1 : 5
        Total_PH(k) = FL_Corner_PH(5-k+1);
    end
    Total_PH(6:20) = Front_PH;          % Offset: 5              
    Total_PH(21:25) = FR_Corner_PH;     % Offset: 20
    Total_PH(26:44) = Right_PH;         % Offset: 25
    Total_PH(45:49) = BR_Corner_PH;     % Offset: 44
    for k = 1 : 15
        Total_PH(49+k) = Back_PH(15-k+1);
    end
    for k = 1 : 5
        Total_PH(64+k) = BL_Corner_PH(5-k+1);
    end
    for k = 1 : 19
        Total_PH(69+k) = Left_PH(19-k+1);
    end    
    Total_PH(89) = 1000;    % Maximum value for bar display

    
%%    
    ph_max = max(Total_PH(1:88));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Validity check for PHs
    Valid_PH = zeros(1, 88);
    for k = 1 : 88
        if Total_PH(k) > 10
            Valid_PH(k) = 1;
        end
    end

    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Display valid PH result on current original image
    Mark_Width = 15;
    
    % FL_Corner : Valid_PH(1:5)
    for x = 1 : Mark_Width
        for line_index = 2 : 5
            if Valid_PH(6 - line_index)
                cur_img(Region_TL_Y+FL_Corner_LUT(1,line_index-1):Region_TL_Y+FL_Corner_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end

    % Front : Valid_PH(6:20)
    for y = 1 : Mark_Width
        for line_index = 2 : 15
            if Valid_PH(line_index+5)
                cur_img(Region_TL_Y+y, Region_TL_X+Front_LUT(1,line_index-1):Region_TL_X+Front_LUT(1,line_index), 1) = 255;
            end
        end
    end
    
    % FR_Corner : Valid_PH(21:25)
    for x = IMG_Width - Mark_Width : IMG_Width
        for line_index = 2 : 5
            if Valid_PH(line_index+20)
                cur_img(Region_TL_Y+FR_Corner_LUT(1,line_index-1):Region_TL_Y+FR_Corner_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end
    
    % Right : Valid_PH(26:44)
    for x = IMG_Width - Mark_Width : IMG_Width
        for line_index = 2 : 19
            if Valid_PH(line_index + 25)
                cur_img(Region_TL_Y+Right_LUT(1,line_index-1):Region_TL_Y+Right_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end
    
    % BR_Corner : Valid_PH(45:49)
    for x = IMG_Width-Mark_Width : IMG_Width
        for line_index = 2 : 5
            if Valid_PH(line_index + 44)
                cur_img(Region_TL_Y+BR_Corner_LUT(1,line_index-1):Region_TL_Y+BR_Corner_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end
    
    % Back : Valid_PH(50:64)
    for y = IMG_Height-Mark_Width : IMG_Height
        for line_index = 2 : 15
            if Valid_PH(65 - line_index)
                cur_img(Region_TL_Y+y, Region_TL_X+Back_LUT(1,line_index-1):Region_TL_X+Back_LUT(1,line_index), 1) = 255;
            end
        end
    end
    
    % BL_Corner : Valid_PH(65:69)
    for x = 1 : Mark_Width
        for line_index = 2 : 5
            if Valid_PH(70 - line_index)
                cur_img(Region_TL_Y+BL_Corner_LUT(1,line_index-1):Region_TL_Y+BL_Corner_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end
    
    % Left : Valid_PH(70:88)
    for x = 1 : Mark_Width
        for line_index = 2 : 19
            if Valid_PH(89 - line_index)
                cur_img(Region_TL_Y+Left_LUT(1,line_index-1):Region_TL_Y+Left_LUT(1,line_index), Region_TL_X+x, 1) = 255;
            end
        end
    end
    

    
%%
    subplot(2,2,1);
    imshow(cur_img); axis on; title('original image');    
    subplot(2,2,2);
    imshow(cur_crop_img); axis on; title('crop&gray image');
    subplot(2,2,3);
    imshow(bin_img); axis on; title(['frame difference image @ ', int2str(i)]);
    subplot(2,2,4);
    bar(Total_PH); title(['Total PH max value : ', int2str(ph_max)]);
    pause(0.1);
    
%     saveas(gcf, figure_name);    
end
